#include <opencv2/opencv.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/tracking.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include "detection.hpp"

#include <iostream>
#include <fstream>
#include <string>


#include "apriltag/operate/apriltag_operate.h"

#include "opentracker/eco/eco.hpp"
#include "opentracker/eco/parameters.hpp"
#include "opentracker/kcf/kcftracker.hpp"

#include <time.h>   // 计时用

#include "PosResolve.h"

using namespace std;
using namespace cv;
using namespace eco;

 #define TIMING // 计时
CamPos DJI_Z3_Pos; // 无人机　在靶标坐标系中的位置 (main)

int main(int argc, char *argv[])
{
    //VideoCapture capture;
    Mat frame;

    //capture.open("/home/gt/桌面/1212_images/2.MP4"); // 1.mp4   2.MP4
    std::string string1 = "/home/gt/video/test1.22./";
    std::string string2;
    // cin >> string2;
    std::string string3 = ".jpg";
    cout<<string1+string2+string3<<endl;
    frame = imread(string1+string2+string3);
    if(frame.empty()) {cout << "empty image\n"; return -1;}




    apriltag apriltag1; // apriltag变量
    apriltag1.init();

    apriltag1.detect(frame);

    if(apriltag1.id != -1)
    {
        apriltag1.describe(frame);
        namedWindow("frame",0);

        Scalar color =Scalar(255,0,0);
        int thickness = 2;
        Point point;
        point.x = frame.cols/2;
        point.y = frame.rows/2;
        int size = 100;
        line(frame,Point(point.x,point.y-size/2),Point(point.x,point.y+size/2),color,thickness,8,0 );
        line(frame,Point(point.x-size/2,point.y),Point(point.x+size/2,point.y),color,thickness,8,0);
        push_feature_points_position(apriltag1.id); //设定靶标在世界坐标系下的位置

        Point2f feature_points[4];
        feature_points[0] = Point2f(apriltag1.p[0][0],apriltag1.p[0][1]);
        feature_points[1] = Point2f(apriltag1.p[1][0],apriltag1.p[1][1]);
        feature_points[2] = Point2f(apriltag1.p[2][0],apriltag1.p[2][1]);
        feature_points[3] = Point2f(apriltag1.p[3][0],apriltag1.p[3][1]);
//        cout<<"position P"<<endl;
//        cout<<feature_points[0].x<<" "<<feature_points[0].y<<endl;
//        cout<<feature_points[1].x<<" "<<feature_points[1].y<<endl;
//        cout<<feature_points[2].x<<" "<<feature_points[2].y<<endl;
//        cout<<feature_points[3].x<<" "<<feature_points[3].y<<endl;

        Mat rvecsMat = Mat(3,1,CV_64FC1, Scalar::all(0));
        Mat tvecsMat = Mat(3,1,CV_64FC1, Scalar::all(0));
        Mat Cam_position = Mat(4,1,CV_64FC1, Scalar::all(0));

        cal_camera_position(feature_points, rvecsMat, tvecsMat, Cam_position); // 计算靶标坐标系下　相机的位置　(存入DJI_Z3_Pos中)

        // 位置输出
        cout << "  pos:" << endl << Cam_position.at<double>(0,0)
             << "  " << -Cam_position.at<double>(0,1)
             << "  " << -Cam_position.at<double>(0,2) << endl;  //为了和gazebo设置坐标相同,y,z加上负号

        // 航向角输出
//        cout << " 相机－地:" << DJI_Z3_Pos.pitch
//             << " 相机－靶标" << DJI_Z3_Pos.yaw
//             << " 无人机－靶标" << DJI_Z3_Pos.roll<<endl;
//        DJI_Z3_Pos.yaw += 90;
        cout << " roll:" << DJI_Z3_Pos.roll
             << " pitch:" << DJI_Z3_Pos.pitch
             << " yaw:" << DJI_Z3_Pos.yaw
             <<endl;
        /*
         * 10 0.1754
         * 15 0.2618
         * 20 0.3491
         * 30 0.5236
         * 45 0.7854
         * 60 1.0472
         * 90 1.5708
         */


    }
    imshow("frame",frame);
    waitKey(0);


//    while(1)
//    {
//        cout << endl << endl << "frame:" << total_frame << "  ";
//#ifdef TIMING
//        double start_time, stop_time;
//        static double fps_;
//        char fpsstring[10];
//        start_time = clock();

//        std::string STRING("FPS:");
//#endif
//        // 寻找靶标
//        {
//            // ************      step 1 跟踪     *************
//            if(apriltag1.count_defind_apriltag >= 0) // 已经确定跟踪的区域
//            {
//                // 初始化 roi
//                if(apriltag1.whether_init_tracker) tracker.init(frame, apriltag1.roi);

//                // 间隔的获取图片
//                for(int i_=0; i_<5; i_++){ capture >> frame; total_frame++;}
//                if(frame.empty()) {cout << "empty image\n"; break;}

////                if(apriltag1.whether_init_tracker) rectangle(frame,apriltag1.roi,Scalar(0,255,255),2,1);// 原始roi 黄色

//                // 跟踪 roi
//                tracker.update(frame, apriltag1.roi);
//                apriltag1.roi_correct(frame); // 矫正roi区域

////                rectangle(frame,apriltag1.roi,Scalar(255,0,0),2,1);// 更新roi 蓝色
//            }
//            else //未　确定跟踪区域　待增加新功能...
//            {
//                for(int i_=0; i_<5; i_++){ capture >> frame; total_frame++;}
//                if(frame.empty()) {cout << "empty image\n"; break;}
//            }

//            // ************        step 2 检测        *************
//            if(apriltag1.whether_init_tracker) // 上次找到了apriltag
//            {
//                // 根据roi,寻找apriltag
//                apriltag1.detect(frame, apriltag1.roi);
//                if(apriltag1.id != -1) // 找到　靶标
//                {
//                    apriltag1.describe(frame);

//                    if(apriltag1.roi.area() < 1200/1.7 && apriltag1.id != 0) // 放大roi
//                        apriltag1.roi_get(1.5*5);
//                    else if(apriltag1.roi.area() > 30000/1.4 && apriltag1.id != 2 && apriltag1.num_apriltag > 1) // 缩小roi
//                        apriltag1.roi_get(1.5/5);
//                    else // 正常检测
//                        apriltag1.roi_get(1.5);

//                    apriltag1.roi_correct(frame);
//                }
//                else
//                    apriltag1.whether_init_tracker = false;
//            }
//            else if(apriltag1.count_defind_apriltag > 5) // 连续n次　没　检测到apriltag了
//            {
//                apriltag1.detect(frame); // 全图搜索 函数
//                if(apriltag1.id != -1)
//                {
//                    apriltag1.describe(frame);

//                    apriltag1.roi_get(1.5);
//                    apriltag1.roi_correct(frame);

//                    apriltag1.count_defind_apriltag = 0;
//                    apriltag1.whether_init_tracker = true;
//                }
//                else{
//                    apriltag1.count_defind_apriltag = -1;
//                    apriltag1.roi_restore(frame);
//                }//全图 未 搜索到  待增加新功能...

//                cout << " *全图搜索*　";
//            }
//            else // 未检测到apriltag，小于n次
//            {
//                if(apriltag1.count_defind_apriltag >= 0) // 为-1时，代表从未找到apriltag的情况
//                    apriltag1.count_defind_apriltag ++;

//                if(apriltag1.roi.area() < 3072) // 暂时放大roi， roi 会被跟踪重置   (放大２ｘ２倍)
//                {
//                    apriltag1.roi_magnify(5);
//                    apriltag1.roi_correct(frame);
//                }

//                apriltag1.detect(frame, apriltag1.roi); // roi搜索目标
//                if(apriltag1.id != -1)
//                {
//                    apriltag1.describe(frame);

//                    apriltag1.roi_get(1.5);
//                    apriltag1.roi_correct(frame);

//                    apriltag1.count_defind_apriltag = 0;
//                    apriltag1.whether_init_tracker = true;
//                }
//            }

//            //　***********        step 3 位置解算        **************
//            if(apriltag1.id != -1)
//            {
//                push_feature_points_position(apriltag1.id); //设定靶标在世界坐标系下的位置

//                Point2f feature_points[4];
//                feature_points[0] = Point2f(apriltag1.p[0][0],apriltag1.p[0][1]);
//                feature_points[1] = Point2f(apriltag1.p[1][0],apriltag1.p[1][1]);
//                feature_points[2] = Point2f(apriltag1.p[2][0],apriltag1.p[2][1]);
//                feature_points[3] = Point2f(apriltag1.p[3][0],apriltag1.p[3][1]);

//                Mat rvecsMat = Mat(3,1,CV_64FC1, Scalar::all(0));
//                Mat tvecsMat = Mat(3,1,CV_64FC1, Scalar::all(0));
//                Mat Cam_position = Mat(4,1,CV_64FC1, Scalar::all(0));

//                cal_camera_position(feature_points, rvecsMat, tvecsMat, Cam_position);

//                cout << "  pos:" << endl << Cam_position.at<double>(0,0) << "  " << Cam_position.at<double>(0,1) << "  " << Cam_position.at<double>(0,2) << endl;
//            }
//        }



//#ifdef TIMING
//        stop_time = clock();
//        fps_ = 0.5*fps_ + 0.5/(((double)(stop_time-start_time))/CLOCKS_PER_SEC);
////        cout << "   fps:" << fps_ << "  " << "time:" << 1/fps_;

//        sprintf(fpsstring,"%.2f",fps_);
//        STRING += fpsstring;
//        putText(frame,STRING,cv::Point(10,100),FONT_HERSHEY_SCRIPT_SIMPLEX,2,Scalar(0,0,255),4);

//#endif

////        cout<<"   面积:"<<apriltag1.roi.area();
////        cout<<"    id:"<<apriltag1.id;

//        imshow("tracker",frame);
//        if(waitKey(30) > 0)
//            break;
//     }

//     apriltag1.destroy(); // 清除动态内存

     return 0;

}

